# -*- coding: utf-8 -*-
# server.py 
from flask import Flask, render_template, jsonify, request
import RPi.GPIO as GPIO
import time
import threading
from _XiaoRGEEK_SERVO_ import XR_Servo
from flask_cors import CORS

app = Flask(__name__)
CORS(app)

#######################################
############# Pin Definitions #########
#######################################
GPIO.setmode(GPIO.BCM)

ENA = 13  # Left motor enable (PWM)
ENB = 20  # Right motor enable (PWM)
IN1 = 19  # Left motor direction 1
IN2 = 16  # Left motor direction 2
IN3 = 21  # Right motor direction 1
IN4 = 26  # Right motor direction 2
LASER_PIN = 7 # laser

# =============== auto running ===============
AUTO_RUNNING = False
TARGET_LOCK = threading.Lock()
current_target = {'x': None, 'y': None, 'radius': 10}  
LASER_POINT_X, LASER_POINT_Y = 320, 360


# Setup GPIO pins
GPIO.setup(ENA, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(ENB, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(LASER_PIN, GPIO.OUT, initial=GPIO.HIGH) # laser

# Create PWM instances at 1000Hz
pwm_A = GPIO.PWM(ENA, 1000)  # Left motor PWM
pwm_B = GPIO.PWM(ENB, 1000)  # Right motor PWM

# Start PWM with 0% duty cycle
pwm_A.start(0)
pwm_B.start(0)
try:
    servo = XR_Servo()
    SERVO_INITIALIZED = True
except Exception as e:
    print("Error:", e)
    SERVO_INITIALIZED = False

# Movement parameters
DEFAULT_SPEED = 50
current_speed = DEFAULT_SPEED
STEP_DURATION = 0.2 


motor_lock = threading.Lock()

def set_speed(speed):
    """Set motor speed via duty cycle"""
    if 0 <= speed <= 100:
        pwm_A.ChangeDutyCycle(speed)
        pwm_B.ChangeDutyCycle(speed)
    else:
        print("Speed must be between 0 and 100")

def Motor_Forward(speed=DEFAULT_SPEED, step_duration=STEP_DURATION):
    with motor_lock:
        print('Motor Forward')
        set_speed(speed)
        GPIO.output(IN1, True)
        GPIO.output(IN2, False)
        GPIO.output(IN3, True)
        GPIO.output(IN4, False)
        time.sleep(step_duration)
        Motor_Stop()

def Motor_Backward(speed=DEFAULT_SPEED, step_duration=STEP_DURATION):
    with motor_lock:
        print('Motor Backward')
        set_speed(speed)
        GPIO.output(IN1, False)
        GPIO.output(IN2, True)
        GPIO.output(IN3, False)
        GPIO.output(IN4, True)
        time.sleep(step_duration)
        Motor_Stop()

def Motor_TurnRight(speed=DEFAULT_SPEED, step_duration=STEP_DURATION):
    with motor_lock:
        print('Motor Turn Right')
        set_speed(speed)
        GPIO.output(IN1, False)
        GPIO.output(IN2, True)
        GPIO.output(IN3, True)
        GPIO.output(IN4, False)
        time.sleep(step_duration)
        Motor_Stop()

def Motor_TurnLeft(speed=DEFAULT_SPEED,step_duration=STEP_DURATION):
    with motor_lock:
        print('Motor Turn Left')
        set_speed(speed)
        GPIO.output(IN1, True)
        GPIO.output(IN2, False)
        GPIO.output(IN3, False)
        GPIO.output(IN4, True)
        time.sleep(step_duration)
        Motor_Stop()
        
def Motor_Stop():
    print('Motor Stop')
    GPIO.output(ENA,True)
    GPIO.output(ENB,True)
    GPIO.output(IN1,False)
    GPIO.output(IN2,False)
    GPIO.output(IN3,False)
    GPIO.output(IN4,False)

def laser_on():
    GPIO.output(LASER_PIN, GPIO.LOW)
    print("laser on")

def laser_off():
    GPIO.output(LASER_PIN, GPIO.HIGH)
    print("laser off")


@app.route('/')
def index():
    return render_template('index.html')

@app.route('/forward')
def forward():
    thread = threading.Thread(target=Motor_Forward, args=(current_speed,))
    thread.start()
    return jsonify({'status': 'moving forward', 'speed': current_speed})

@app.route('/backward')
def backward():
    thread = threading.Thread(target=Motor_Backward, args=(current_speed,))
    thread.start()
    return jsonify({'status': 'moving backward', 'speed': current_speed})

@app.route('/left')
def turn_left():
    thread = threading.Thread(target=Motor_TurnLeft, args=(current_speed,))
    thread.start()
    return jsonify({'status': 'turning left', 'speed': current_speed})

@app.route('/right')
def turn_right():
    thread = threading.Thread(target=Motor_TurnRight, args=(current_speed,))
    thread.start()
    return jsonify({'status': 'turning right', 'speed': current_speed})

@app.route('/stop')
def stop():
    Motor_Stop()
    return jsonify({'status': 'stopped'})

@app.route('/laser_on')
def laser_on_api():
    laser_on()
    return jsonify({'status': 'laser on'})

@app.route('/laser_off')
def laser_off_api():
    laser_off()
    return jsonify({'status': 'laser off'})

@app.route('/set_speed', methods=['POST'])
def set_speed_api():
    global current_speed
    data = request.get_json()
    print("data:", data) 
    speed = data.get('speed', DEFAULT_SPEED)
    if 0 <= speed <= 100:
        current_speed = speed
        #set_speed(speed)
        return jsonify({'status': 'success', 'speed': current_speed})
    else:
        return jsonify({'status': 'error', 'message': 'Speed must be between 0 and 100'})

@app.route('/set_servo', methods=['POST'])
def set_servo_api():
    if not SERVO_INITIALIZED:
        return jsonify({'status': 'error', 'message': 'Servo not initialized'})
    
    try:
        data = request.get_json()
        print("data:",data)
        servo_num = data.get('servo_num', 1)
        angle = data.get('angle', 90)
        
        if 0 <= angle <= 180:
            servo.XiaoRGEEK_SetServoAngle(servo_num, angle)
            return jsonify({'status': 'success', 'servo': servo_num, 'angle': angle})
        else:
            return jsonify({'status': 'error', 'message': 'Angle must be between 0 and 180'})
    except Exception as e:
        return jsonify({'status': 'error', 'message': str(e)})

@app.route('/move', methods=['POST'])
def move():
    data = request.get_json()
    direction = data.get('dir')
    speed = data.get('speed', current_speed)
    step = data.get('step', STEP_DURATION)

    if direction == 'forward':
        thread = threading.Thread(target=Motor_Forward, args=(speed, step))
    elif direction == 'backward':
        thread = threading.Thread(target=Motor_Backward, args=(speed, step))
    elif direction == 'left':
        thread = threading.Thread(target=Motor_TurnLeft, args=(speed, step))
    elif direction == 'right':
        thread = threading.Thread(target=Motor_TurnRight, args=(speed, step))
    else:
        return jsonify({'status': 'error', 'message': 'Invalid direction'}), 400

    thread.start()
    return jsonify({'status': 'moving', 'direction': direction, 'speed': speed, 'step': step})
    
@app.route('/auto_stop')
def auto_stop():
    global AUTO_RUNNING
    AUTO_RUNNING = False
    Motor_Stop()
    laser_off()
    return jsonify({'status': 'auto stopped'})

@app.route('/target', methods=['POST'])
def receive_target():
    global current_target, AUTO_RUNNING
    if not AUTO_RUNNING:
        return jsonify({'status': 'ignored', 'reason': 'auto not running'})

    data = request.get_json()
    x = data.get('x')
    y = data.get('y')
    radius = data.get('radius', 10)

    if x is not None and y is not None:
        with TARGET_LOCK:
            current_target['x'] = x
            current_target['y'] = y
            current_target['radius'] = radius
        return jsonify({'status': 'target updated', 'x': x, 'y': y})
    else:
        return jsonify({'status': 'error', 'message': 'missing x or y'})
    
def cleanup():
    """Clean up GPIO settings"""
    pwm_A.stop()
    pwm_B.stop()
    laser_off()
    GPIO.cleanup()
    print("GPIO cleaned up")

if __name__ == '__main__':
    try:
        app.run(host='0.0.0.0', port=5000, debug=False)
    except KeyboardInterrupt:
        print("\nProgram interrupted by user")
    finally:
        cleanup()
